# !/usr/bin/env python3
# -*- coding: utf-8 -*-
# @Time    : 2025/07/21 16:00
# @Author  : Yida Hao
# @File    : get_joint_states.py

from rclpy.node import Node
from sensor_msgs.msg import JointState
from interfaces.srv import GetJointStates
from typing import Optional

SRV_NAME = 'get_joint_state_service'

"""
Service to get joint states from the robot.
"""

class GetJointStateService(Node):
    def __init__(self):
        super().__init__(f'{SRV_NAME}_srv')

        self.init_joint_state_sub()

        self.current_joint_state = JointState()

        self.srv = self.create_service(GetJointStates, SRV_NAME, self.srv_callback)

        self.get_logger().info("service ready")

    def srv_callback(self, request, response):
        response.joint_state = self.current_joint_state
        self.get_logger().info(f'service called, got joint position: {response.joint_state.position}')
        return response
        
    def init_joint_state_sub(self):
        self.joint_state_subscriber = self.create_subscription(
            JointState,
            'joint_states',
            self.joint_state_callback,
            10
        )

    def joint_state_callback(self, msg: JointState):
        self.current_joint_state = msg  


class GetJointStateClient(Node):
    def __init__(self):
        super().__init__(f'{SRV_NAME}_cli')
        self.cli = self.create_client(GetJointStates, SRV_NAME)

        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting ...')

        self.req = GetJointStates.Request()

    def send_request(self):
        future = self.cli.call_async(self.req)
        return future

